// $Id$
// Implementation of planetarydynamicsworld
//
// ml278
// Copyright (C) 2008 Bjørn Øivind Bjørnsen, Aleksander Gjermundsen, Knut Andre Tidemann, Thomas Hansen
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

#include "planetarydynamicsworld.h"

#include <LinearMath/btQuickprof.h>
#include <btBulletDynamicsCommon.h>
#include <Ogre.h>

using namespace Ogre;

PlanetaryDynamicsWorld::PlanetaryDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) :
		btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration)
{
}

PlanetaryDynamicsWorld::~PlanetaryDynamicsWorld()
{
}

/// updates gravity of all active rigidbodies based on proximity to celestial bodies
void PlanetaryDynamicsWorld::predictUnconstraintMotionAndGravity(btScalar timeStep)
{
	for ( int i=0;i<m_collisionObjects.size();i++ )
	{
		btCollisionObject* colObj = m_collisionObjects[i];
		btRigidBody* body = btRigidBody::upcast ( colObj );
		if ( body )
		{
			if ( !body->isStaticOrKinematicObject() )
			{
				if ( body->isActive() )
				{
					// get position
					// check proximity to celestial bodies
					// apply gravity based on formula
					btVector3 gravity(0,0,0);
					
					for(unsigned int j = 0; j < mPlanetaryBodies.size(); j++)
					{
						btVector3 planetaryBodyPos(mPlanetaryBodies[j]->mPosition.x,
								mPlanetaryBodies[j]->mPosition.y,
								mPlanetaryBodies[j]->mPosition.z);
						btVector3 bodyPos = body->getCenterOfMassPosition();
						btVector3 direction = planetaryBodyPos - bodyPos;
						direction.normalize();
						
						Real bodyMass = Math::Pow(body->getInvMass(), -1);
						
						btScalar gravityConstant = 6.67e-11;
						
						btScalar accel = gravityConstant * (mPlanetaryBodies[j]->mMass);
						accel = accel / bodyPos.distance2(planetaryBodyPos);
						
						// gravitational constant * (mass1 * mass2)/distance²
						//btScalar force = 6.67e-10 * ((mPlanetaryBodies[j]->mMass * bodyMass) / bodyPos.distance2(planetaryBodyPos));
						direction *= accel;
						gravity += direction;
					}
					
					body->setGravity(gravity);
					
					body->applyGravity();
					body->applyDamping( timeStep);
					body->integrateVelocities( timeStep);
					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
				}
			}
		}
	}
}

/// overridden method from btDiscreteDynamicsWorld, overridden to provide dynamic gravity
void PlanetaryDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
	startProfiling ( timeStep );
	
	updateAabbs();
	
	// update gravity for all active bodies and predict unconstraint motion
	predictUnconstraintMotionAndGravity(timeStep);
	
	//predictUnconstraintMotion ( timeStep );
	
	btDispatcherInfo& dispatchInfo = getDispatchInfo();
	
	dispatchInfo.m_timeStep = timeStep;
	dispatchInfo.m_stepCount = 0;
	dispatchInfo.m_debugDraw = getDebugDrawer();
	
	performDiscreteCollisionDetection();
	
	calculateSimulationIslands();
	
	getSolverInfo().m_timeStep = timeStep;
	
	solveConstraints ( getSolverInfo() );
	
	integrateTransforms ( timeStep );
	
	updateVehicles ( timeStep );
	
	updateActivationState ( timeStep );
}

/// sets the celestial bodies to consider when applying gravity
void PlanetaryDynamicsWorld::setBodies(std::vector< PlanetaryBody * > bodies)
{
	mPlanetaryBodies = bodies;
}



